 /*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package org.homullus.SkeletonTransforms;

import org.homullus.Skeleton;
import org.homullus.Bone;
import org.homullus.Pose;
import org.homullus.PoseDirection;

/**
 *
 * @author Gary
 */
public class PoseSkeleton implements ISkeletonTransform
{
    private Pose lastPose = null;

    public void setLastPose(Pose pose) { this.lastPose = pose; }

    private Pose pose = null;
    /**
     * @return the pose
     */
    public Pose getPose() { return pose; }

    /**
     * @param pose the pose to set
     */
    public void setPose(Pose pose) { this.pose = pose; }


    public void Transform(Skeleton skel)
    {
        if (pose != null)
        {
            ApplyPose(pose.getRootPose(),(lastPose != null ? lastPose.getRootPose() : null)  ,skel.GetRoot());
            lastPose = pose;
        }
    }

    public void ApplyPose(PoseDirection p, PoseDirection l, Bone b)
    {
        if (b.editable)
        {
            RotateBone(b,CalculateAngle(p.getPitch() - (l != null ? l.getPitch() : 0)),
                         CalculateAngle(p.getYaw() - (l != null ? l.getYaw() : 0)),
                         CalculateAngle(p.getRole() - (l != null ? l.getRole() : 0)));
        }
        if (b.childBones != null)
        {
            for (int cb = 0; cb < b.childBones.size(); cb++)
            {
                ApplyPose(p.getChildPose().get(cb),(l != null ? l.getChildPose().get(cb) : null), b.childBones.get(cb));
            }
        }
    }

    private float CalculateAngle(float val)
    {
        return (val * 360)/1023;
    }

    private String ftos(float f)
    {
        return Float.toString(f);
    }
    

    public void RotateBone(Bone b, float x, float y, float z)
    {
        if (b.Parent != null)
        {
            //System.out.println("Rotating bone: " + b.Name);
            //System.out.println("-------------------------------------");
            //System.out.println("angle: " + x + ", " + y + ", " + z);
            //System.out.println("before: " + b.x + ", " + b.y + ", " + b.z);
            RotateSubBones(b.Parent, b, x, y, z);
        }
    }

    private void RotateSubBones(Bone root, Bone b, double xang, double yang, double zang)
    {
        //System.out.println("yang " + Double.toString(yang));
        //System.out.println("xang " + Double.toString(xang));
        Double x = new Double(0);
        Double y = new Double(0);
        Values v = new Values();
        //compute the y rotation for x is x and z is y
        if (yang != 0)
        {
            Rotate(root.x, root.z, b.x, b.z, yang, v);
            System.out.println("x: " + x.toString());
            System.out.println("y: " + y.toString());
            b.x = v.GetX();
            b.z = v.GetY();
        }
        //<editor-fold>
        /*
        double a = Math.atan2(b.z - root.z, b.x - root.x);
        if (a < 0)
        {
            a = (2*Math.PI) + a;
        }
        double dist = Math.hypot(root.x - b.x, root.z - b.z);
        double yvect = 0;//dist * Math.sin(Math.toRadians(yang) + a);
        double xvect = 0;// dist * Math.cos(Math.toRadians(yang) + a);
        if (yang > 0)
        {
            double angle = Math.toRadians(yang);
            double finalangle = angle + a;
            double sin = Math.sin(finalangle);
            double cos = Math.cos(finalangle);
            xvect = dist * Math.cos(angle + a);
            yvect = dist * Math.sin(angle + a);
            b.x = (float)xvect;
            b.z = (float)yvect;
        }
        else
        {
            double angle = Math.toRadians(-yang);
            double finalangle = angle - a;
            double sin = Math.sin(finalangle);
            double cos = Math.cos(finalangle);
            xvect = dist * Math.cos(angle - a);
            yvect = dist * Math.sin(angle - a);
            b.x = (float)xvect;
            b.z = (float)yvect * -1.0f;
        }
         */
        //</editor-fold>
        
        //compute the x rotation for z is x and y is y
        if (xang != 0)
        {
            Rotate(root.z, root.y, b.z, b.y, xang, v);
            b.z = v.GetX();
            b.y = v.GetY();
        }
        //<editor-fold>
        /*
        a = Math.atan2(root.y - b.y, root.z - b.z);
        dist = Math.hypot(root.z - b.z, root.y - b.y);
        if (yang > 0)
            yvect = dist * Math.sin(Math.toRadians(yang) + a);
        else
            yvect = dist * Math.sin(Math.toRadians(-yang) - a);
        if (xang > 0)
            xvect = dist * Math.cos(Math.toRadians(yang) + a);
        else
            xvect = dist * Math.cos(Math.toRadians(-yang) - a);

        b.z = (float)xvect;// * -1.0f;
        b.y = (float)yvect;// * -1.0f;
        */
        //</editor-fold>
        //compute the z rotation for x is x and y is y
        if (zang != 0)
        {
            Rotate(root.x, root.y, b.x, b.y, zang, v);
            b.x = v.GetX();
            b.y = v.GetY();
        }
        if (b.childBones != null)
        {
            for (int sb = 0; sb < b.childBones.size(); sb++)
            {
                RotateSubBones(root ,b.childBones.get(sb),xang, yang, zang);
            }
        }
    }

    private void Rotate(float cx, float cy, float x, float y,double angle, Values v)
    {
        double a = Math.atan2(y - cy, x - cx);
        if (a < 0)
        {
            a = (2*Math.PI) + a;
        }
        double dist = Math.hypot(cx - x, cy - y);
        if (angle > 0)
        {
            double finalangle = Math.toRadians(angle) + a;
            v.SetX(dist * Math.cos(finalangle));
            v.SetY(dist * Math.sin(finalangle));
        }
        else
        {
            double finalangle = Math.toRadians(-angle) - a;
            v.SetX(dist * Math.cos(finalangle));
            v.SetY(dist * Math.sin(finalangle) * -1.0f);
        }
    }

    protected class Values
    {
        private Double x = new Double(0);
        public void SetX(double v) { x = v; }
        public float GetX() { return x.floatValue(); }

        private Double y = new Double(0);
        public void SetY(double v) { y = v; }
        public float GetY() { return y.floatValue(); }

        public Values() {};
    }

}
